#!/usr/bin/env python

""" patch_tracker.py - Version 0.1 2011-06-08

    Looks for ROI' on the "input" topic, then publishes the corresponding
    ROI's on the /masks topic.

    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2011 Patrick Goebel.  All rights reserved.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.5
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses/gpl.html
      
"""

import roslib
roslib.load_manifest('pi_rein')
import rospy
import sys
from pi_rein.msg import *
from pi_rein.srv import *

class PatchTracker:
    def __init__(self):
        
        rospy.init_node("patch_tracker")
        
        self.patch_pub = rospy.Publisher("masks", MaskArray)
        self.patch_sub = rospy.Subscriber("input", DetectionArray, self.patch_callback)
        
        rospy.wait_for_message("input", DetectionArray, timeout=15)

    def patch_callback(self, detections):
        masks = self.convert_detections_to_masks(detections)
        self.patch_pub.publish(masks)
        
    def convert_detections_to_masks(self, detections):
        masks = MaskArray()
        for detection in detections.detections:
            mask = detection.mask
            masks.masks.append(mask)
        masks.header = detections.header
        return masks

def main(args):
    # Display a help message if appropriate.
    help_message = ""
          
    print help_message
    
    try:
        PatchTracker()
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down tracker node."

if __name__ == '__main__':
    main(sys.argv)